package com.cosyhut.blocks.jode;

import java.util.logging.Level;

import com.jme.math.*;
import com.jme.util.LoggingSystem;
import com.cosyhut.blocks.factory.*;

public class ODEJoint extends ODEJointPoint {
	protected SWIGTYPE_p_dxJoint _joint;

	protected MJoint _model;

	public ODEJoint(MJoint model) {
		super();
		this._model = model;
	}

	public void connect(ODEJointPoint jp) {
		//if not attach to a block, then cannot connect to other joint
		if (null == this._block)
			return;
		if (null == jp._block)
			return;

		// create and set joint
		createJoint(this._block.getWorld(), this._block.getGroup());
		//!! must attach a joint first before setting the geom params !!
		jode.dJointAttach(this._joint, this._block.getBody(), jp._block
				.getBody());

		Vector3f anchor = this.getWorldPosition();
		this.setAnchor(anchor);
		Vector3f axis = new Vector3f();
		axis.set(1, 0, 0);
		this.setAxis(this.getWorldRotation().mult(axis));
		axis.set(0, 1, 0);
		this.setAxis2(this.getWorldRotation().mult(axis));

		this.setParam(jodeConstants.dParamVel, 1f);
		this.setParam(jodeConstants.dParamFMax, 100f);
	}

	// -------------------------------------------------------------------------
	private void createJoint(SWIGTYPE_p_dxWorld world,
			SWIGTYPE_p_dxJointGroup group) {
		if (jodeConstants.dJointTypeHinge == this._model.getType()) {
//			MJointHinge m = (MJointHinge) this._model;
			this._joint = jode.dJointCreateHinge(world, group);
		} else {
    		LoggingSystem.getLogger().log(Level.WARNING, "trying to create unknown joint model\n");
    		return;
		}
	}

	private void setAnchor(Vector3f anchor) {
		if (jodeConstants.dJointTypeHinge == this._model.getType()) {
			Vector3f a = this._model.getAnchor();
			jode.dJointSetHingeAnchor(this._joint, a.x, a.y, a.z);
		} else {
			assert (false);
		}
	}

	private void setAxis(Vector3f axis) {
		if (jodeConstants.dJointTypeHinge == this._model.getType()) {
			Vector3f a = ((MJointHinge)(this._model)).getAxis();
			jode.dJointSetHingeAxis(this._joint, a.x, a.y, a.z);
		} else {
			assert (false);
		}

	}

	private void setAxis2(Vector3f axis) {
		if (jodeConstants.dJointTypeHinge == this._model.getType()) {
			// no axis2 for joint type hinge
			return;
		} else {
			assert (false);
		}
		
	}

	private void setParam(int param, float value) {
		if (jodeConstants.dJointTypeHinge == this._model.getType()) {
			jode.dJointSetHingeParam(this._joint, param, value);
		} else {
			assert (false);
		}
		
	}
	// -------------------------------------------------------------------------
	public MJoint getModel() {
		return this._model;
	}
}
